#include <iostream>
#include <vector>
#include <queue>
#include <cmath>

struct Node
{
    int x, y;
    int f, g ,h;
    Node* parent;
    bool operator < (const Node& rhs) const{
        return f > rhs.f;
    }
};

class JumpPointSearch{
public:
    JumpPointSearch(int n,int m,std::vector<std::vector<int>>& maze,
                    int startX,int startY,int endX,int endY){
        rows = n;
        cols = m;
        start = new Node{startX,startY,0,0,0,nullptr};
        end = new Node{endX,endY,0,0,0,nullptr};
        grid = maze;
        visited.resize(rows,std::vector<bool>(cols,false));
    }

    // 执行路径搜索
    std::vector<std::pair<int, int>> search(){
        std::priority_queue<Node> pq;
        pq.push(*start);
        visited[start->x][start->y] = true;

        while (!pq.empty())
        {
            Node cur = pq.top();
            pq.pop();

            if(cur.x == end->x && cur.y == end->y){
                return  generatePath(cur);
            }

            std::vector<Node*> neighbors = findNeighbors(cur);
            for(Node* nb : neighbors){
                if(visited[nb->x][nb->y]){
                    continue;
                }
                visited[nb->x][nb->y] = true;
                nb->parent = &cur;
                nb->g = cur.g + getDistance(cur, *nb);
                nb->h = getDistance(*nb, *end);
                nb->f = nb->g + nb->h;
                pq.push(*nb);
            } 
        }
        return std::vector<std::pair<int,int>>();
    }
private:
    int rows, cols;
    Node* start,* end;
    std::vector<std::vector<int>> grid;
    std::vector<std::vector<bool>> visited;

    // 获取两个节点之间的曼哈顿距离
    int getDistance(const Node& a,const Node& b){
        return abs(a.x - b.x) + abs(a.y - b.y);
    }

    // 判断一个节点是否在地图内
    bool isValid(int x, int y){
        return x >= 0 && x < rows && y >= 0 && y < cols && grid[x][y] == 0;
    }

    // 查找关键节点，返回为空表示这是一个普通的节点
    Node* jump(int cx, int cy, int px, int py){
        int dx = cx - px;
        int dy = cy - py;
        if(!isValid(cx, cy)){
            return nullptr;
        }

        if(cx == end->x && cy == end->y){
            return end;
        }
        
        if(dx != 0 && dy != 0){
            // 斜向移动
            if((isValid(cx + dx,cy + dy) && !isValid(cx, cy + dy)) || 
                (isValid(cx + dx,cy + dy) && !isValid(cx + dx, cy))){
                    return new Node{cx, cy, 0,0,0,nullptr};
                }
            Node* toward = jump(cx + dx,cy,cx,cy);
            Node* diag = jump(cx, cy + dy,cx,cy);
            if(toward || diag){
                return new Node{cx,cy,0,0,0,nullptr};
            }
        } else{

        }
    }

    std::vector<std::pair<int, int>> generatePath(Node cur);
};

#include "JPS.hpp"
#include <iostream>
#include <functional>
#include <math>
#include <climits>
namespace searchway{
int JumpPointSearch::get_h(point start,point end){
    return (start.x - end.x)*(start.x - end.x) + (start.y - end.y)*(start.y - end.y);
}

std::vector<point> JumpPointSearch::findneighbours(point p){
    std::vector<point> point_v;
    point parent = p.parent;
    if(parent == null){
        // 起点
        for(int x=-1;x<=1;x++){
            for(int y=-1;y<=1;y++){
                if(x == 0 && y == 0){
                    return 0;
                }
                if(this->map(x+p.x,y+p.y) == true){
                    point_v.push_back({p.x+x,p.y+y,0,0,0});
                }
            }
        }
        return point_v
    }

    // 非起点邻居点判断
    int xDirection = p.x - parent.x;
    if(xDirection < 0){
        xDirection = -1;
    }
    if(xDirection > 0){
        xDirection = 1;
    }
    int yDirection = p.y - parent.y;
    if(yDirection < 0){
        yDirection = -1;
    }
    if(yDirection > 0){
        yDirection = 1;
    }
    if(xDirection != 0 && yDirection != 0){
        // 对角方向
        bool neighbourForward = this->map(p.x,p.y + yDirection);
        bool neighbourBack    = this->map(p.x,p.y - yDirection);
        bool neighbourRight   = this->map(p.x + xDirection,p.y);
        bool neighbourLeft    = this->map(p.x - xDirection,p.y);
        if (neighbourForward) {
            points.push_back({p.x,p.y + yDirection,0,0,0});
        }
        if (neighbourRight){
            points.push_back({p.x + xDirection,p.y,0,0,0});
        }
        if((neighbourForward || neighbourRight) && \
            this->map(p.x + xDirection,p.y + yDirection)){
            points.push_back({p.x + xDirection,p.y + yDirection,0,0,0});
        }
        // 强迫邻居的处理
        // oo*o
        // ooxo
        // oooo
        if(!neighbourLeft && neighbourForward){
            if(this->map(p.x - xDirection,p.y + yDirection)){
                points.push_back({p.x - xDirection,p.y + yDirection,0,0,0});
            }
        }
        if(!neighbourBack && neighbourRight){
            if(this->map(p.x + xDirection,p.y - yDirection)){
                points.push_back({p.x + xDirection,p.y - yDirection,0,0,0});
            }
        }
    } else {
        if(xDirection == 0){
            if(this->map(p.x, p.y + yDirection)){
                points.push_back({p.x, p.y + yDirection});// 原本方向保留
                // 强迫邻居
                if(!this->map(p.x+1,p.y)&&this->map(p.x+1,p.y + yDirection)){
                    points.push_back({p.x+1,p.y + yDirection});
                }
                if(!this->map(p.x-1,p.y)&&this->map(p.x-1,p.y + yDirection)){
                    points.push_back({p.x-1,p.y + yDirection})
                }
            }
        } else {
            if(this->map(p.x + xDirection, p.y)){
                points.push_back({p.x + xDirection,p.y});
                // 强迫邻居
                if(!this->map(p.x,p.y+1) && this->map(p.x + xDirection,p.y + 1)){
                    points.push_back({p.x + xDirection,p.y + 1});
                }
                if(!this->map(p.x,p.y-1) && this->map(p.x + xDirection,p.y - 1)){
                    points.push_back({p.x + xDirection,p.y - 1})
                }
            }
        }
    }
    return points;
}

point JumpPointSearch::is_jump_point(\
                    int curPosx,int curPosy,\
                    int xDirection,int yDirection,\
                    int depth,\
                    point end){
    if(this->map == NULL){
        return false;
    }
    if(this->map(curPosx,curPosy) == false){
        return false;
    }
    // 递归到最大深度 || 搜索到终点
    if(depth == 0 || end.x == curPosx && end.y == curPosy){
        return point(curPosx,curPosy)
    }
    // 对角向
    if(xDirection != 0 && yDirection != 0){
        if((this->map(curPosx + xDirection,curPosx - yDirection)&&!this->map(curPosx,curPosy - yDirection))||\
            (this->map(curPosx - xDirection,curPosx + yDirection)&&!this->map(curPosx - xDirection,curPosy))){
                return point(curPosx,curPosy);
            }
        if(is_jump_point(curPosx + xDirection,curPosy,xDirection,0,depth-1,end)!=NULL){
            return point(curPosx,curPosy);
        }
        if(is_jump_point(curPosx,curPosy + yDirection,0,yDirection,depth-1,end)!=NULL){
            return point(curPosx,curPosy);
        }
    } else {
        if(xDirection == 0){
            if((this->map(curPosx + 1,curPosy + yDirection)&&!this->map(curPosx+1,curPosy))||\
                (this->map(curPosx - 1,curPosy + yDirection)&&!this->map(curPosx-1,curPosy))){
                return point(curPosx,curPosy);
            }
        }
        if(yDirection == 0){
            if((this->map(curPosx + xDirection,curPosy + 1)&&!this->map(curPosx,curPosy+1))||\
            (this->map(curPosx + xDirection,curPosy - 1)&&!this->map(curPosx,curPosy-1))){
                return point(curPosx,curPosy);
            }
        }
    }
    return is_jump_point(curPosx + xDirection,curPosy + yDirection,xDirection,yDirection,depth-1,end);
}

int JumpPointSearch::CalcG(point start,point point){
    int distX = abs(start.x - point.x);
    int distY = abs(start.y - point.y);
    int G = 0;
    if(distX > distY){
        G = 14 * distY + 10 * (distX - distY);
    } else {
        G = 14 * distX + 10 * (distY - distY);
    }
    int parentG = point.parent != NULL ? point.parent.g : 0;
    return G + parentG;
}

int JumpPointSearch::CalcH(point end,point point){
    int step = abs(point.x - end.x) + abs(point.y - end.y);
    return step * 10;
}

std::vector<std::vector<int>> JumpPointSearch::make_close_map(int num){
    std::vector<std::vector<int>> close_map(height,std::vector<int>(width,num));
    return close_map;
}

point JumpPointSearch::findway(point start, point end,blocked_func blocked_func){
    std::priority_queue<point> q;
    q.push(start{x,y,0,0,get_h(start,end)});
    std::vector<std::vector<int>> close_map = this->make_close_map(0);
    std::vector<std::vector<int>> realF = this->make_close_map(INT_MAX);
    while(!q.empty()){
        point tmp = q.top();
        close_map[tmp.x][tmp.y] = tmp.f;
        if(tmp.x == end.x && tmp.y == end.y){
            return true;
        }
        q.pop();
        std::vector<point> neigbours = this->findneighbours(tmp);
        for(auto neighbor : neighbours){
            point jp = \
                this->is_jump_point(neighbor.x,neighbor.y,\
                                    neighbor.x - tmp.x,neighbor.y - tmp.y\
                                    100,end);
            // 当前点没有被访问过or代价小于原来的值
            if(close_map[jp.x][jp.y] != 1 or tmp.g + this->CalcG(tmp,jp) < realF[jp.x][jp.y]){
                jp.g = this->CalcG(tmp,jp) + tmp.g;
                jp.h = this->CalcH(end,jp);
                jp.f = jp.g + jp.h;
                jp.parent = tmp;
                realF[jp.x][jp.y] = jp.g;
                q.push(jp);
            }
        }
    }
    return point;
}

}